#!/usr/bin/env python
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person

import rospy
import serial
from text_pkg.msg import cmd_vel

def personInfoCallback(msg):
    ser = serial.Serial('/dev/ttyUSB0', 9600, timeout=10)
    data =bytes('p1'+str(msg.pwm1)+'p2'+str(msg.pwm2)+'b1'+str(msg.back1)+'b2'+str(msg.back2)+
    'q1'+str(msg.quit1)+'q2'+str(msg.quit2)+'\r\n')
    ser.write(data)
    ser.close()
    rospy.loginfo("STM32 subscrib [p1:%d,p2:%d,b1:%d,b2:%d,q1:%d,q2:%d]", 
				msg.pwm1,msg.pwm2,msg.back1,msg.back2,msg.quit1,msg.quit2)
    

def subscriber():
	# ROS节点初始化
    rospy.init_node('STM32')

	# 创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personInfoCallback
    rospy.Subscriber("/cmd_vel", cmd_vel, personInfoCallback)

	# 循环等待回调函数
    rospy.spin()

if __name__ == '__main__':
   subscriber()
